// Main task code with hardware handler functions and the major software code
// Team 4 in Senior Design
// Based off the task design from TI

/* ========= Includes =========== */

// General include for MSP430
#include <msp430.h>

// includes for running SYS/BIOS
#include <xdc/std.h>
#include <xdc/runtime/Log.h>
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/knl/Semaphore.h>

// includes for linking in project code
#include "bike_data.h"
#include "display.h"
#include "GlobalVars.h"
#include "GSGCdef.h"
#include "ANT_Utils.h"
#include "ANT.h"

//ANT Messages
#define ANT_MESG_LENGTH 40
extern char ANTReadMessage[ANT_MESG_LENGTH]; //Latest message 
                        //from and using ANTMesgSem fro MUTEX
// Forward declarations
void setup(void);

/*
 *  ======== main ========
 */
Void main()
{
    // Uncomment this next line to initialize the port direction for the LED.
    // P1DIR |= 0x1;

    // Print to log file
    Log_info0("Hello world starting main\n");

    /* 
     * Start BIOS.
     * Begins task scheduling.
     */
    Log_info0("Starting SYS/BIOS\n");
    BIOS_start();        /* does not return */
}

/*
 *  ======== TimerHandler ========
 * Hardware interrupt run every 50,000 milliseconds (set in task.cfg)
 * uses post to semaphores to control the running of DisplayMain
 * and DataMain
 */
Void TimerHandler(UArg arg)
{
    // Uncomment this next line to toggle the LED state.
    // P1OUT ^= 0x1;

    DisplayCount += 1;
    DataCount += 1;
    WirelessCount += 1;

    // every 40 timer interrupts let DisplayMain start
    // 100 * 5,000 milliseconds = 0.5 second period
    if (DisplayCount  > (100 -1)) {
      DisplayCount = 0;
      Semaphore_post(DisplaySem);
    }

    // every 20 timer interrupts let DisplayMain start
    // 2 * 5,000 milliseconds = .1 second period
    if (DataCount  > (20 -1)) {
      DataCount = 0;
      Semaphore_post(DataSem);
    }

    // every 20 timer interrupts let DisplayMain start
    // 2 * 5,000 milliseconds = .1 second period
    if (WirelessCount  > (20 - 1)) {
      WirelessCount = 0;
      Semaphore_post(WirelessSem);
    }

}

/*
 *  ======== Uart0Int ========
 * Hardware interrupt when there is a receive on the UART0
 */

Void Uart0Int(Void){
  //check that the interrupt was for a positive edge, if so post to the
  //semaphore and reset the interrupt
  //Log_info0("Inside UART0 interrupt");
  if((UCA0IFG & UCRXIFG)){
    Semaphore_post(UART0Sem);
    UCA0IFG &= ~UCRXIFG;
  }
}

/*
 *  ======== Uart3Int ========
 * Hardware interrupt when there is a receive on the UART3
 */

Void Uart3Int(Void){
  //check that the interrupt was for a positive edge, if so post to the
  //semaphore and reset the interrupt
  //Log_info0("Inside UART3 interrupt");
  if((UCA3IFG & UCRXIFG)){
    Semaphore_post(UART3Sem);
    UCA3IFG &= ~UCRXIFG;
  }
}

/*
 * ========= UART3Fxn ==========
 * Function to handle freading in from the UART
 */

Void UART3Fxn(Void) {
  Log_info0("Starting UART3Fxn");
  int length = 20; //Lenggth of the current message
  int count = 0; //Current place being read to 
  char mesg; //Current message read
  while(TRUE){
    // wait till a new message comes in
    Semaphore_pend(UART3Sem, BIOS_WAIT_FOREVER);

    mesg = UCA3RXBUF; //Read in the new value
    //Log_info1("UART3 saw 0x%X",mesg);

    if(count >= length){
      if((mesg == 0xA4) || (mesg == 0xA5)) //New Sync Byte
        count = 0;
      else
        continue; //Restart waiting for next bit
    }

    //the lenght of ANT message is four plus the length
    //value for Sync, Mesg Length, Mesg ID & Checksum bytes
    if(count == 1)
      length = mesg + 4;

    if(count == ANT_MESG_LENGTH)
      continue; //message to long. cut oof end

    // Use the semaphore to provide mutual exclusion
    Semaphore_pend(ANTMesgSem, BIOS_WAIT_FOREVER);
      ANTReadMessage[count] = mesg;
    Semaphore_post(ANTMesgSem); //End of Mutex

    count++;
    if(count == length)
      Semaphore_post(NewANTMesgSem); //A new message has been decode
  }
}

/*
 *  ======== DisplayMain ========
 *  The main thread that controls the running of the OLED display
 */
Void DisplayMain(Void)
{
  // Call setup to initialize variables and setup for each thread
  setup();

  Log_info0("Starting Display Main");
  int count = 0;
  int hours, min;
  long int sec,distRaw;
  int spdCur, hbpmCur, calRaw, cadRaw;
  int hbpmAvg, cadAvg, spdAvg;
  int hbpmMax, cadMax, spdMax;
  int curScreen;
  char flag;

  while (TRUE) {
    // Wait until the hardware wakes up function
    Log_info0("Posting on DisplaySem in Display Main");
    Semaphore_pend(DisplaySem, BIOS_WAIT_FOREVER);

    count++;
    //Log_info0("In the DisplayMain\n");

    // Take in data from Data Structure
    Log_info0("Posting on DataStrucSem in Display Main");
    Semaphore_pend(DataStrucSem, BIOS_WAIT_FOREVER);
      hours = getElapsedTimeHour(&BikeDataStruc);
      min = getElapsedTimeMin(&BikeDataStruc);
      sec = (long int) getElapsedTimeMilSec(&BikeDataStruc)/1000;
      distRaw = 100*DistanceTraveledMiles(getWheelRevolutionCount(&BikeDataStruc));
      spdCur = getSpeed(&BikeDataStruc);
      hbpmCur = getHeartRate(&BikeDataStruc);
      calRaw = getCalories(&BikeDataStruc);
      cadRaw = getCadence(&BikeDataStruc);
      hbpmAvg = getAvgHeartRate(&BikeDataStruc);
      spdAvg = getAvgSpeed(&BikeDataStruc);
      cadAvg = getAvgCadence(&BikeDataStruc);
      hbpmMax = getMaxHeartRate(&BikeDataStruc);
      spdMax = getMaxSpeed(&BikeDataStruc);
      cadMax = getMaxCadence(&BikeDataStruc);
    Semaphore_post(DataStrucSem);

    Log_info0("Posting on TempDataSem in Display Main");
    Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
      flag = alertByte;
      curScreen = Screen;
    Semaphore_post(TempDataSem);

    Log_info3("hours = %d, min = %d, sec = %d",hours,min,sec);

    //Print to the display
    //screen6(sec,hbpmMax,spdMax,cadMax,flag);
    //Log_info2("hbpmCur = %d calRaw = %d",hbpmCur,calRaw);
      
    //Set Screen based off of rotary encoder
    Log_info1("Screen number %d",curScreen);
    switch(curScreen){
      case 1:
      screen1(hours,min,sec,spdCur,distRaw,flag);
      //Log_info2("spdCur = %d distRaw = %d",spdCur,distRaw);
      break;
      case 2:
      screen2(hours,min,sec,hbpmCur,calRaw,flag);
      //Log_info2("hbpmCur = %d calRaw = %d",hbpmCur,calRaw);
      break;
      case 3:
      screen3(hours,min,sec,cadRaw,distRaw,flag);
      //Log_info2("cadRaw = %d distRaw = %d",cadRaw,distRaw);
      break;
      case 4:
      screen4(sec,hbpmCur,spdCur,cadRaw,flag);
      //Log_info3("hbpmCur = %d spdRaw = %d cadRaw = %d",hbpmCur,spdCur,cadRaw);
      break;
      case 5:
      screen5(sec,hbpmAvg,spdAvg,cadAvg,flag);
      //Log_info3("hbpmAvg = %d spdAvg = %d cadAvg = %d",hbpmAvg,spdAvg,cadAvg);
      break;
      case 6:
      screen6(sec,hbpmMax,spdMax,cadMax,flag);
      //Log_info3("hbpmMax = %d spdMax = %d cadMax = %d",hbpmMax,spdMax,cadMax);
      break;
      default:
      screen1(hours,min,sec,spdCur,distRaw,flag);
      //Log_info2("Screen 1 by default spdCur = %d distRaw = %d",spdCur,distRaw);
      break;
    }

      
    //Cycle through screens
    /*if(count < 10){
      screen1(hours,min,sec,spdCur,distRaw,flag);
      Log_info2("spdCur = %d distRaw = %d",spdCur,distRaw);
    }
    else if(count < 20){
      screen2(hours,min,sec,hbpmCur,calRaw,flag);
      Log_info2("hbpmCur = %d calRaw = %d",hbpmCur,calRaw);
    }
    else if(count < 30){
      flag |= ALERTLEFT;
      screen3(hours,min,sec,cadRaw,distRaw,flag);
      Log_info2("cadRaw = %d distRaw = %d",cadRaw,distRaw);
    }
    else if(count < 40){
      flag |= ALERTMID;
      screen4(sec,hbpmCur,spdCur,cadRaw,flag);
      Log_info3("hbpmCur = %d spdRaw = %d cadRaw = %d",hbpmCur,spdCur,cadRaw);
    }
    else if(count < 50){
      flag |= ALERTRIGHT;
      screen5(sec,hbpmAvg,spdAvg,cadAvg,flag);
      Log_info3("hbpmAvg = %d spdAvg = %d cadAvg = %d",hbpmAvg,spdAvg,cadAvg);
    }
    else{
      flag |= ALERTRIGHT | ALERTMID | ALERTLEFT;
      screen6(sec,hbpmMax,spdMax,cadMax,flag);
      Log_info3("hbpmMax = %d spdMax = %d cadMax = %d",hbpmMax,spdMax,cadMax);
      if(count == 59)
        count = 0;
    }*/
  }
}

/*
 *  ======== DataMain ========
 *  The main thread that controls the updating of the Data Structure
 *  ************************* !!!! IMPORTANT !!!! **********************
 *  Calculatioins inside are baised off the assumptioin that the function
 *  runs once every second
 */
Void DataMain(Void)
{
  Log_info0("Starting Data Main");
  //Test variable
  int count = 0;
  // Internal variables for calculating update values
  float newSpeed, newCadence, newHeartRate;
  int newUpdateWheelCount;
  //int newUpdatePedalCount,newUpdateHeartCount;

  // For testing initialize values
  Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);

    //20 mile an hour speed
    //WheelSPR = (float) 0.23644;
    Speed = 7;
    Cadence  = (float) 90;
    //HeartRate = 150;
    updateWheelCount = 3;
    updatePedalCount = 1;
    updateHeartCount = 1;

  Semaphore_post(TempDataSem);

    while (TRUE) {
        // Wait until the hardware wakes up function
        Semaphore_pend(DataSem, BIOS_WAIT_FOREVER);

        // Get data from temporary storage
        Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);

          //Read in values
          newSpeed = Speed;
          newCadence = Cadence;
          newHeartRate = HeartRate;
          //Manually update for test
          newUpdateWheelCount = updateWheelCount;
          //newUpdatePedalCount = updatePedalCount;
          //newUpdateHeartCount = updateHeartCount;

        Semaphore_post(TempDataSem);

        /*if(count > 2){
          newUpdateWheelCount = 1;
          count = 0;
        }
        else
          newUpdateWheelCount = 0;*/

        count++;

        //update the data structure
        Semaphore_pend(DataStrucSem, BIOS_WAIT_FOREVER);
          //Baised on runing once a second
          update(&BikeDataStruc,newSpeed,newCadence,newHeartRate,newUpdateWheelCount,100);
        Semaphore_post(DataStrucSem);

    }
}

/* 
 * ========== WirelessMain =============
 * The main thread for the wireless function handleing the 
 * comunication over ANT+
 */

Void WirelessMain(Void) {
  int result;
  unsigned int cadTime,cadCount,spdTime,spdCount;
  unsigned int oldCadTime,oldCadCount,oldSpdTime,oldSpdCount;
  unsigned int diffCadTime,diffCadCount,diffSpdTime,diffSpdCount;
  int firstRun = 1;
  int oldProxEventCount = 0;
  Log_info0("Starting WirelessMain");
  //Constants
  const float wheelSize = 2098.58;
  const float mmPerMile = 1.609344e6;


  // Setup ANT
  UART_ANT_init();

  //Software reset of ANT
  ResetAnt();

  //Setup ANT Heart Rate channel
  char ucTransType = 0;
  char usDeviceNum[2] = {0,0};
  result = ConfigureRxChannelHRM(ucTransType,usDeviceNum);
  if(!result){
    Log_info0("Heart Rate Channel set up\n");
    Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
      alertByte |= CONNHR;
    Semaphore_post(TempDataSem);
  }
  else{
    Log_info1("Heart Rate Channel fail %d\n", result);
    Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
      alertByte &= ~CONNHR;
    Semaphore_post(TempDataSem);
  }

  //Setup ANT Cadence channel
  result = ConfigureRxChannelCAD(ucTransType,usDeviceNum);
  if(!result){
    Log_info0("Cadence Channel set up\n");
    Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
      alertByte |= CONNCAD;
    Semaphore_post(TempDataSem);
  }
  else{
    Log_info1("Cadence Channel fail %d\n", result);
    Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
      alertByte &= ~CONNCAD;
    Semaphore_post(TempDataSem);
  }

  //Setup ANT Prox channel
  result = ConfigureRxChannelPROX(ucTransType,usDeviceNum);
  if(!result){
    Log_info0("PROX Channel set up\n");
    Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
      alertByte |= CONNPROX;
    Semaphore_post(TempDataSem);
  }
  else{
    Log_info1("PROX Channel fail %d\n", result);
    Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
      alertByte &= ~CONNPROX;
    Semaphore_post(TempDataSem);
  }

  char message[13];
  while(TRUE) {
    // Pend on WirelessSem until the timer ISR says its time
    // to do something Now just use the read_ANT to block till new message
    //Semaphore_pend(WirelessSem, BIOS_WAIT_FOREVER);

    // read a value from ANT+
    read_ANT(message,9);
    Log_info2("Msg ID 0x%X Channel num 0x%X",message[2],message[3]);
    if(message[2] == 0x4E){ //Broadcast message received
      switch(message[3]){ //Switch based on channel number
        case HR_CHANNEL: //Heart Rate Message
          Log_info1("HR %d", message[11]);
          Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
            HeartRate = message[11];
            alertByte |= CONNHR; //Heart Rate is connected
          Semaphore_post(TempDataSem);
        break;

        case CAD_CHANNEL: //Cadence Sensor Message
          cadTime = message[4] + (message[5]<<8);
          //Log_info2("message[4] = 0x%X message[5] = 0x%X",message[4],message[5]);
          cadCount = message[6] + (message[7]<<8);
          //Log_info2("message[6] = 0x%X message[7] = 0x%X",message[6],message[7]);
          spdTime = message[8] + (message[9]<<8);
          //Log_info2("message[8] = 0x%X message[9] = 0x%X",message[8],message[9]);
          spdCount = message[10] + (message[11]<<8);
          //Log_info2("message[10] = 0x%X message[11] = 0x%X",message[10],message[11]);
          /*Log_info2("Cadence Time = %u Cadence Count = %u",cadTime,cadCount);
          Log_info2("Old Cadence Time = %u Old Cadence Count = %u",oldCadTime, oldCadCount);
          Log_info2("Speed Time = %u Speed Count = %u",spdTime,spdCount);
          Log_info2("Old Speed Time = %u Old Speed Count = %u",oldSpdTime,oldSpdCount);*/

          if(firstRun)//First run just update old values
            firstRun = 0;
          else{ //Not first event so update Speed and Cadence

            //Calculate the differences between old and new
            if(oldCadTime <= cadTime) //Normal case
              diffCadTime = cadTime - oldCadTime;
            else //Rolled over
              diffCadTime = cadTime + 65536 - oldCadTime;

            if(oldCadCount <= cadCount) //Normal case
              diffCadCount = cadCount - oldCadCount;
            else //Rolled over
              diffCadCount = cadCount + 65536 - oldCadCount;

            if(oldSpdTime <= spdTime) //Normal case
              diffSpdTime = spdTime - oldSpdTime;
            else //Rolled over
              diffSpdTime = spdTime + 65536 - oldSpdTime;

            if(oldSpdCount <= spdCount) //Normal case
              diffSpdCount = spdCount - oldSpdCount;
            else //Rolled over
              diffSpdCount = spdCount + 65536 - oldSpdCount;

            /*Log_info2("diffCadTime = %u diffCadCount = %u",diffCadTime,diffCadCount);
            Log_info2("diffSpdTime = %u diffSpdCount = %u",diffSpdTime,diffSpdCount);*/

            //Calculate the Speed, Cadence and wheel count
            //values
            Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
              if(diffCadTime)//make sure no divide by zero
                Cadence = (float)60*diffCadCount*1024/diffCadTime;
              if(diffSpdTime)//make sure no divide by zero
                Speed = (float) wheelSize/mmPerMile*3600*diffSpdCount*1024/diffSpdTime*10;

              updateWheelCount += diffSpdCount;
              
              alertByte |= CONNCAD;//Cadence is connected
              Log_info3("Speed = %d Cadence = %d updateWheelCount = %d",Speed,Cadence,updateWheelCount);
            Semaphore_post(TempDataSem);
          }

          //Update old values
          oldCadTime = cadTime;
          oldCadCount = cadCount;
          oldSpdTime = spdTime;
          oldSpdCount = spdCount;
        break;

        case PROX_CHANNEL: //A message from the Proximity sensor
          Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
            //update Aleart bits
            Log_info3("message[9] = 0x%X, message[10] = 0x%X, message[11] = 0x%X",message[9],message[10],message[11]);
            if(message[9])
              alertByte |= ALERTLEFT;
            else
              alertByte &= ~ALERTLEFT;

            if(message[10])
              alertByte |= ALERTMID;
            else
              alertByte &= ~ALERTMID;

            if(message[11])
              alertByte |= ALERTRIGHT;
            else
              alertByte &= ~ALERTRIGHT;
              
            //update Current Screen
            Log_info2("message[7] = 0x%X, message[8] = 0x%x",message[7],message[8]);
            if((message[7] == 2) || (message[8] == 1))
              Screen++; //rotation or button push changes to next screen
            if(message[7] == 1)
              Screen--; //rotation to previous screen

              //Handle rapping around the screen
            if(Screen < 1)
              Screen = 6; //zero raps around to 6
            if(Screen > 6)
              Screen = 1; //seven raps around to 1

            alertByte |= CONNPROX;//Prox is connected
            Log_info2("New AlertByte = 0x%X Screen = %d\n",alertByte,Screen);
          Semaphore_post(TempDataSem);
        break;
      }
    }
    else if(message[2] == 0x40){ //Channel Event message
      Log_info3("Mesg ID 0x%X Channel num = 0x%X and Error Code %d", message[2],message[3],message[5]);
      if(message[5] == 0x1){//Time out Event
        switch(message[3]){
          case HR_CHANNEL: //Heart Rate Message No Longer Connected
            Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
              alertByte &= ~CONNHR;
            Semaphore_post(TempDataSem);
          break;
          case CAD_CHANNEL: //Cadence & Speed channel No Longer Connected
            Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
              alertByte &= ~CONNCAD;
            Semaphore_post(TempDataSem);
          break;
          case PROX_CHANNEL: //PROX channel No longer connected
            Semaphore_pend(TempDataSem, BIOS_WAIT_FOREVER);
              alertByte &= ~CONNPROX;
            Semaphore_post(TempDataSem);
          break;
        }
        reconnectANT(message[3]); //Re open the channel
      }
    }
    else
      Log_info1("Mesg ID 0x%X unrecognized data", message[2]);
  }
}

/*
 * ========= setup ===========
 * initializes internal variables and calls the individual
 * setups for each of the threads
 */
Void setup(void){
  Log_info0("Starting Setup of Global values\n");

  // Initialize counts to zero so they can be used in hardware handler
  DisplayCount = 0;
  DataCount = 0;
  WirelessCount = 0;

  // Initialize the BikeDataStruc and set up personal data
  Semaphore_pend(DataStrucSem, BIOS_WAIT_FOREVER);
    initializeBikeData(&BikeDataStruc);
    setPersonalData(&BikeDataStruc, 25, 170,MALE);
  Semaphore_post(DataStrucSem);

  // Initialize the Temporary storage
  /*Speed = 0;
  Cadence = 0;
  HeartRate = 0;*/
  Speed = 7;
  Cadence = 90;
  HeartRate = 0;

  // Setup the display code
  UART_OLED_init(); //Setup registers for UART
  send_auto_baud(1000); //Send auto baud, waiting 1000 times
  clearScreen(); //clear the screen of the display
  bitmaps(); //sets the custom bitmaps

  Log_info0("Finishing Setup of Global values\n");
}
